/**
 * @file	localization_node.cpp
 * @author	George Andrew Brindeiro
 * @date	04/10/2012
 *
 * @attention Copyright (C) 2012
 * @attention Laboratório de Automação e Robótica (LARA)
 * @attention Universidade de Brasília (UnB)
 */

#include <artoolkit_localization/localization_node.h>
#include <artoolkit_localization/ukf.h>

// Standard C libraries
#include <csignal>
#include <cstdio>

// ROS libraries
#include <ros/ros.h>

#include <ar_pose/ARMarkers.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_broadcaster.h>

std::vector<double> controls;
std::map< int, vector<double> > measurements;

UKF* localizer;

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

void publish_map_markers(ros::Publisher& marker_pub)
{
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "/odom";
    marker.header.stamp = ros::Time::now();

	// Set the namespace and id for this marker.  This serves to create a unique ID
	// Any marker sent with the same namespace and id will overwrite the old one
	marker.ns = "map";
	marker.id = 4;

	// Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
	marker.type = visualization_msgs::Marker::CUBE;

	// Set the marker action.  Options are ADD and DELETE
	marker.action = visualization_msgs::Marker::ADD;

	// Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
	marker.pose.position.x = 1.8;
	marker.pose.position.y = 0;
	marker.pose.position.z = 2.75;
	marker.pose.orientation.x = 0.0;
	marker.pose.orientation.y = 0.0;
	marker.pose.orientation.z = 0.0;
	marker.pose.orientation.w = 1.0;

	// Set the scale of the marker -- 1x1x1 here means 1m on a side
	marker.scale.x = 0.13;
	marker.scale.y = 0.13;
	marker.scale.z = 0.03;

	// Set the color -- be sure to set alpha to something non-zero!
	marker.color.r = 1.0f;
	marker.color.g = 0.0f;
	marker.color.b = 1.0f;
	marker.color.a = 1.0;

	marker.lifetime = ros::Duration();

    // Publish the marker
    marker_pub.publish(marker);
}

int main(int argc, char **argv)
{
    // Setup signal handlers
    setup_sig_handler();

    /* Node initialization */

    // Setup ROS structures
    ros::init(argc, argv, "localization_node");
    ros::NodeHandle loc_nh;
    
    // Subscribed topics
    
    ros::Subscriber sub_odom = loc_nh.subscribe<nav_msgs::Odometry>("pose", 1, twist_callback);
    ros::Subscriber sub_markers = loc_nh.subscribe<ar_pose::ARMarkers>("ar_pose_marker", 1, ar_pose_marker_callback);
    
    // Published topics

    //ros::Publisher pub_pose = loc_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("artoolkit_localization/pose", 1);
    ros::Publisher pub_pose_no_cov = loc_nh.advertise<geometry_msgs::PoseStamped>("artoolkit_localization/pose_no_cov", 1);

    // Broadcasted camera/robot transform

    tf::Transform transform;
    tf::TransformBroadcaster br;

	// Node loop rate

    ros::Rate loop_rate(100);

    /* UKF */
    ROS_DEBUG("Path to ARMap is not absolute... Possible SEGFAULT!");
    localizer = new UKF(0.1, "cfg/lara.map");

    /* MAP MARKERS */
    ros::Publisher marker_pub = loc_nh.advertise<visualization_msgs::Marker>("visualization_marker", 1);

    while(ros::ok())
    {
        ros::Time ts = ros::Time::now();

        /* Node main loop */
    	if(controls.size() > 0 || measurements.size() > 0)
    	{
    		localizer->localize(controls,measurements);
        	controls.clear();
        	measurements.clear();
    	}
		else
			ROS_DEBUG("No controls or measurements available.");

        // Publish current pose estimate
        //pub_pose.publish(localizer->pose_msg());
        pub_pose_no_cov.publish(localizer->pose_msg().pose);

        //publish_map_markers(marker_pub);

        // odom to pose_no_cov
        btQuaternion rotation;
        tf::quaternionMsgToTF(localizer->pose_msg().pose.pose.orientation, rotation);

        transform.setOrigin( tf::Vector3(localizer->pose_msg().pose.pose.position.x, localizer->pose_msg().pose.pose.position.y, 0) );
        transform.setRotation(rotation);
        br.sendTransform(tf::StampedTransform(transform, ts, "odom", "pose_no_cov"));

    	// Publish world/camera tf transform based on robot position
    	// Height: 0.85 Rotation: 90º in x
        transform.setOrigin( tf::Vector3(0, 0, 0.85) );
        transform.setRotation( tf::createQuaternionFromRPY(-1.57, 0, 0) );
        br.sendTransform(tf::StampedTransform(transform, ts, "base_link", "camera"));

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

void twist_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
	controls.clear();

	ROS_DEBUG("Received twist message!");

	double v = msg->twist.twist.linear.x;
	double w = msg->twist.twist.angular.z;

	controls.push_back(v);
	controls.push_back(w);

	ROS_DEBUG("(v,w) = (%lf,%lf)", v, w);
}

void ar_pose_marker_callback(const ar_pose::ARMarkers::ConstPtr& msg)
{
	measurements.clear();

	std::vector<ar_pose::ARMarker> markers = msg->markers;
	
	int num_markers = markers.size();
	
	if(num_markers > 0)
	{
		if(num_markers == 1)
			ROS_DEBUG("Received %d ar_pose_marker message!", num_markers);
		else
			ROS_DEBUG("Received %d ar_pose_marker messages!", num_markers);
		
		for(std::vector<ar_pose::ARMarker>::iterator marker = markers.begin(); marker != markers.end(); ++marker)
		{
			int num_marker = marker-markers.begin();
			uint32_t id = marker->id;
			double dx = marker->pose.pose.position.y; // OBS: inversion due to difference in camera/body frame orientation
			double dy = -marker->pose.pose.position.x;
			double theta = localizer->state(2,0);

			measurements[id].push_back(UKF::range(dx,dy));
			measurements[id].push_back(UKF::bearing(dx,dy,theta));

			ROS_DEBUG("Marker #%d", num_marker);
			ROS_DEBUG("id: %d", id);
			ROS_DEBUG("Position: (%lf,%lf)", dx, dy);
			ROS_DEBUG("Range/Bearing: (%lf,%lf)", measurements[id][0], measurements[id][1]);
		}
	}
}

void setup_sig_handler()
{
    signal(SIGSEGV, &sig_handler);
    signal(SIGINT, &sig_handler);
    signal(SIGTSTP, &sig_handler);
}

void sig_handler(int sig)
{
    switch(sig)
    {
        case SIGSEGV:
            signal(SIGSEGV, SIG_DFL);
            printf("Signal caught: SIGSEGV\n");
            break;
        case SIGINT:
            signal(SIGINT, SIG_IGN);
            printf("Signal caught: SIGINT\n");
            break;
        case SIGTSTP:
            signal(SIGTSTP, SIG_IGN);
            printf("Signal caught: SIGTSTP\n");
            break;
    }

    printf("Closing artoolkit_localization nicely...\n");
    
    exit(0);
}
